/**
  ******************************************************************************
  * File Name          : motor.c
  * Description        : This file provides code for the configuration
  *                      of the TiMotor instances.
  ******************************************************************************
  * @attention
  * Created by Chen Lu in 20210504
  *
  ******************************************************************************
  */

#include "motor.h"
#include "stdlib.h"
#include "string.h"
#include "tim.h"

/**
 * ********************************************
 * In this project, we use PWM to power motors.
 *
 * Mapping between motors and TIMER channels:
 * motor_X	<--->	TIM2_CH2(PA1)
 * motor_Y	<--->	TIM3_CH2(PA7)
 * motor_Z	<--->	TIM4_CH4(PB9)
 *
 * Mapping between motor enable and GPIO pin:
 * motor_EN(X/Y/Z share)	<--->	PB10
 *
 * Mapping between motor direction and GPIO pin:
 * motor_X_dir	<--->	PB11
 * motor_Y_dir	<--->	PA4
 * motor_Z_dir	<--->	PA5
 *
 * Mapping between motor zero and GPIO pin:
 * motor_X_zero	<--->	(It does not have zero adjustment, so it need to be adjusted manually.)
 * motor_Y_zero	<--->	PC8
 * motor_Y_zero	<--->	PC9
 * ********************************************
 */

/**
  * @brief  Stop all motors.
  *
  * @note   (Nothing important)
  *
  * @param  None
  * @retval MOTOR status
  */
uint8_t motor_stop_all()
{
	if (HAL_TIM_PWM_Stop(&htim2,TIM_CHANNEL_2)!=HAL_OK)
	{
		printf("%s\r\n","Motor stop TIM2 PWM error!");
		return MOTOR_ERROR;
	}
	if (HAL_TIM_PWM_Stop(&htim3,TIM_CHANNEL_2)!=HAL_OK)
	{
		printf("%s\r\n","Motor stop TIM3 PWM error!");
		return MOTOR_ERROR;
	}
	if (HAL_TIM_PWM_Stop(&htim4,TIM_CHANNEL_4)!=HAL_OK)
	{
		printf("%s\r\n","Motor stop TIM4 PWM error!");
		return MOTOR_ERROR;
	}
	return MOTOR_OK;
}

/**
  * @brief  Initiate all motors.
  *
  * @note   This function stops all motors and disable all motors.
  *
  * @param  None
  * @retval MOTOR status
  */
uint8_t motor_init()
{
	HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port,MOTOR_EN,SET);
	if (motor_stop_all()!=MOTOR_OK)
	{
		printf("%s\r\n","Motor stop all error!");
		return MOTOR_ERROR;
	}
	return MOTOR_OK;
}

/**
  * @brief  Initiate a motor instance.
  *
  * @note   This function creates a motor instance.
  *
  * @param  id	The id number of the motor to be initiated.
  * @retval A motor instance pointer.
  */
TiMotor * motor_open(uint8_t id)
{
	TiMotor *motor = (TiMotor*) malloc(sizeof(TiMotor));
	memset(motor, 0x00, sizeof(TiMotor));
	motor->id = id;
	HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port, MOTOR_EN, RESET);
	return motor;
}

/**
  * @brief  Close a motor instance.
  *
  * @note   This function closes a motor instance(disable it actually), before which the motor will be stopped.
  *
  * @param  motor	The motor instance pointer.
  * @retval MOTOR status.
  */
uint8_t motor_close(TiMotor* motor)
{
	switch (motor->id)
	{
		case 0:
			if (HAL_TIM_PWM_Stop(&htim2,TIM_CHANNEL_2)!=HAL_OK)
			{
				printf("%s\r\n","Motor stop TIM2 PWM error!");
				return MOTOR_ERROR;
			}
			break;
		case 1:
			if (HAL_TIM_PWM_Stop(&htim3,TIM_CHANNEL_2)!=HAL_OK)
			{
				printf("%s\r\n","Motor stop TIM3 PWM error!");
				return MOTOR_ERROR;
			}
			break;
		case 2:
			if (HAL_TIM_PWM_Stop(&htim4,TIM_CHANNEL_4)!=HAL_OK)
			{
				printf("%s\r\n","Motor stop TIM4 PWM error!");
				return MOTOR_ERROR;
			}
			break;
		default:
			printf("%s\r\n","Motor id wrong!");
			break;
	}
	HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port,MOTOR_EN,SET);

	return MOTOR_OK;
}

/**
  * @brief  Configure a motor instance.
  *
  * @note   This function sets direction and speed of a motor instance.
  *
  * @param  motor		The motor instance pointer.
  * @param  direction	Direction of the motor.
  * 			@arg 'P':	Positive direction.
  * 			@arg 'N':	Negative direction.
  * @param  speed		Speed of the motor. Unit: pulses / s
  * @retval MOTOR status.
  */
uint8_t motor_write(TiMotor* motor,uint8_t direction,uint16_t speed)
{
	(motor->direction)=direction;
	(motor->speed)=speed;
	return MOTOR_OK;
}

/**
  * @brief  Start a motor.
  *
  * @note   This function start a motor by set ARR&CNT of a timer according to the motor speed.
  * 		This way to set PWM frequency is a no choice measure(we have to access register directly,
  * 		where no measures provided in HAL library), cause the motor provided in this project is a stepper motor.
  *
  * @param  motor		The motor instance pointer.
  * @retval MOTOR status.
  */
uint8_t motor_start(TiMotor* motor)
{
	switch (motor->id)
	{
		case 0:
			if ((motor->direction)=='P')
				HAL_GPIO_WritePin(MOTOR_X_DIR_GPIO_Port,MOTOR_X_DIR,SET);
			else if ((motor->direction)=='N')
				HAL_GPIO_WritePin(MOTOR_X_DIR_GPIO_Port,MOTOR_X_DIR,RESET);
			else
				return MOTOR_ERROR;

			TIM2->ARR = CNT_FREQ / motor->speed;
			TIM2->CCR2 = CNT_FREQ / motor->speed / 2;

			if (HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2)!=HAL_OK)
			{
				printf("%s\r\n","Motor start TIM2 PWM error!");
				return MOTOR_ERROR;
			}

			break;
		case 1:
			if ((motor->direction)=='P')
				HAL_GPIO_WritePin(MOTOR_Y_DIR_GPIO_Port,MOTOR_Y_DIR,SET);
			else if ((motor->direction)=='N')
				HAL_GPIO_WritePin(MOTOR_Y_DIR_GPIO_Port,MOTOR_Y_DIR,RESET);
			else
				return MOTOR_ERROR;

			TIM3->ARR = CNT_FREQ / motor->speed;
			TIM3->CCR2 = CNT_FREQ / motor->speed / 2;

			if (HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2)!=HAL_OK)
			{
				printf("%s\r\n","Motor start TIM3 PWM error!");
				return MOTOR_ERROR;
			}

			break;
		case 2:
			if ((motor->direction)=='P')
				HAL_GPIO_WritePin(MOTOR_Z_DIR_GPIO_Port,MOTOR_Z_DIR,SET);
			else if ((motor->direction)=='N')
				HAL_GPIO_WritePin(MOTOR_Z_DIR_GPIO_Port,MOTOR_Z_DIR,RESET);
			else
				return MOTOR_ERROR;

			TIM4->ARR = CNT_FREQ / motor->speed;
			TIM4->CCR4 = CNT_FREQ / motor->speed / 2;

			if (HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4)!=HAL_OK)
			{
				printf("%s\r\n","Motor start TIM4 PWM error!");
				return MOTOR_ERROR;
			}
			break;
		default:
			break;
	}

	return MOTOR_OK;
}

/**
  * @brief  Stop a motor.
  *
  * @note   (Nothing important)
  *
  * @param  motor		The motor instance pointer.
  * @retval MOTOR status.
  */
uint8_t motor_stop(TiMotor* motor)
{
	switch (motor->id)
	{
		case 0:
			if (HAL_TIM_PWM_Stop(&htim2,TIM_CHANNEL_2)!=HAL_OK)
			{
				printf("%s\r\n","Motor stop TIM2 PWM error!");
				return MOTOR_ERROR;
			}
			break;
		case 1:
			if (HAL_TIM_PWM_Stop(&htim3,TIM_CHANNEL_2)!=HAL_OK)
			{
				printf("%s\r\n","Motor stop TIM3 PWM error!");
				return MOTOR_ERROR;
			}
			break;
		case 2:
			if (HAL_TIM_PWM_Stop(&htim4,TIM_CHANNEL_4)!=HAL_OK)
			{
				printf("%s\r\n","Motor stop TIM4 PWM error!");
				return MOTOR_ERROR;
			}
			break;
		default:
			printf("%s\r\n","Motor id wrong!");
			break;
	}

	return MOTOR_OK;
}



